﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using System.Threading;

namespace ERF.Physics
{
    /// <summary>
    /// The main physics engine.
    /// </summary>
    public class PhysicsEngine
    {
        private bool isRunning = false;
        private bool pendingStop = false;
        private Thread workerThread;
        private SynchronizedCollection<Body> bodies;
        private List<PhysicsLogic> logic;
        internal IHeightmapScene scene;
        private Plane[] collisionPlanes;
        private AutoResetEvent resetEvent;
        public AutoResetEvent physicsResetEvent;


        /// <summary>
        /// Defines the different types of ray intersections.
        /// </summary>
        public enum RayIntersectionType
        {
            None,
            Body,
            Terrain,
        }

        /// <summary>
        /// Creates a new instance of the PhysicsEngine class.
        /// </summary>
        /// <param name="game">The game that this instance belongs to.</param>
        public PhysicsEngine(ERFGame game, AutoResetEvent resetEvent)
        {
            this.physicsResetEvent = new AutoResetEvent(false);
            this.resetEvent = resetEvent;
            game.Services.AddService(typeof(PhysicsEngine), this);

            this.bodies = new SynchronizedCollection<Body>();
            this.logic = new List<PhysicsLogic>();
        }

        /// <summary>
        /// Starts the execution of the physics engine logic.
        /// </summary>
        public void Start()
        {
            //if (this.isRunning)
            //    throw new InvalidOperationException("Physics engine already running.");
            if (!this.isRunning)
            {
                this.isRunning = true;
                this.workerThread = new Thread(this.Update);
                this.workerThread.IsBackground = true;
                this.workerThread.Start();
            }
        }

        /// <summary>
        /// Stops the execution of the physics engine logic.
        /// </summary>
        public void Stop()
        {
            if (!this.isRunning)
                throw new InvalidOperationException("Physics engine isn't running.");

            // Signal the resetevent so that the physics engine does not wait for the Update-thread to signal before stopping.
            if (this.resetEvent != null)
                this.resetEvent.Set();

            this.pendingStop = true;
        }

        /// <summary>
        /// Adds a body to this physics engine.
        /// </summary>
        /// <param name="body">The body to add to the engine.</param>
        public void AddBody(Body body)
        {
            this.bodies.Add(body);
        }

        /// <summary>
        /// Remove a body to this physics engine.
        /// </summary>
        /// <param name="body">The body to add to the engine.</param>
        public void RemoveBody(Body body)
        {
            this.bodies.Remove(body);
        }

        public void RemoveAllBodies()
        {
            this.bodies.Clear();
        }

        /// <summary>
        /// Adds logic to the physics engine. The PhysicsLogic instances added to this engine will 
        /// be used to alter the bodies each time this PhysicsEngines Update is called, in the order that
        /// the PhysicsLogics where added to the engine.
        /// </summary>
        /// <param name="logic">The PhysicsLogic instance to add.</param>
        public void AddLogic(PhysicsLogic logic)
        {
            logic.parentEngine = this;
            this.logic.Add(logic);
        }

        /// <summary>
        /// Gets or sets the scene on which this PhysicsEngine will operate.
        /// </summary>
        public IHeightmapScene Scene
        {
            get { return this.scene; }
            set
            {
                this.scene = value;
                this.scene.GetCollisionPlanes(out this.collisionPlanes);
            }
        }

        /// <summary>
        /// Gets a value denoting if the physics logics is running or not.
        /// </summary>
        public bool IsRunning
        {
            get { return this.isRunning;  }
        }

        private void Update()
        {
            DateTime lastUpdate = DateTime.Now;
            float timeSinceLastUpdate = 0;

            do
            {
                // Apply any changes made to the body collection before we start.
                this.bodies.ApplyChanges();

                // Calculate the time in seconds since the last update.
                timeSinceLastUpdate = (float)(DateTime.Now - lastUpdate).TotalSeconds;
                lastUpdate = DateTime.Now;

                // Apply the physics logics to all bodies.
                this.ApplyLogics(timeSinceLastUpdate);

                // Calculate new positions for all bodies.
                this.CalculatePosition(timeSinceLastUpdate);

                // Check for collisions.
                this.CollisionCheck();

                // Raise the BodyChanged event for all bodies that have been altered during this update.
                for (int i = 0; i < this.bodies.Count; i++)
                {
                    // stateChanged will be true if the bodys position has been changed during this update.
                    if (this.bodies[i].stateChanged)
                    {
                        this.bodies[i].RaiseBodyChanged();
                        this.bodies[i].stateChanged = false;
                    }

                    // Resets the bodys net forces back to zero.
                    bodies[i].force = Vector3.Zero;
                }

                // If we are not asked to stop the update loop, we should signal to the main thread that we have reached the end
                // of our update cycle, and wait for the main thread to signal that it is ready.
                if (!this.pendingStop)
                {
                    this.physicsResetEvent.Set();
                    this.resetEvent.WaitOne();
                }

            } while (!this.pendingStop);

            this.isRunning = false;
        }

        /// <summary>
        /// Performs a ray intersection test on the IHeightmapScene of this PhysicsEngine, and as the Bodies
        /// registered in it.
        /// </summary>
        /// <param name="ray">The ray to perform the intersection test with.</param>
        /// <param name="emittingBody">
        /// The body that emitts the ray. Used for avoiding collisions with the emitting body.
        /// Can be null.
        /// </param>
        /// <param name="maxDistance">
        /// The max distance to check for collision within.
        /// Use float.PositiveInfinity or float.NegativeInfinity for infinite distance rays.</param>
        /// <param name="intersectingBody">The intersecting body, if any.</param>
        /// <param name="terrainIntersection">The position on the scene that the ray intersects, if any.</param>
        /// <returns>Returns the type of intersection.</returns>
        public RayIntersectionType RayIntersection(ref Ray ray, Body emittingBody, float maxDistance, out Body intersectingBody, out Vector3? terrainIntersection)
        {
            float? bodyCollisionDistance = null;
            float? distance;

            terrainIntersection = null;
            intersectingBody = null;

            // Check every body registered in the engine to see which one it intersects first.
            for (int i = 0; i < this.bodies.Count; i++)
            {
                if (this.bodies[i] != emittingBody)
                {
                    BoundingBox box = new BoundingBox(
                        this.bodies[i].boundingBox.Min + this.bodies[i].position,
                        this.bodies[i].boundingBox.Max + this.bodies[i].position);

                    box.Intersects(ref ray, out distance);
                    if (((bodyCollisionDistance == null && distance != null) ||     // If minDistance is null, meaning that there has been no other body collision yet,
                        // And distance is not null, meaning that the current body is colliding with the ray...
                        (distance != null && distance < bodyCollisionDistance)) &&  // OR, there has already been a collision, but this collision is closer to the start point.
                        (distance <= maxDistance))                     // AND the distance at which the body and ray collide is less than the maxDistance of the ray.
                    {
                        // We have found the "best" collision for now :)
                        bodyCollisionDistance = distance;
                        intersectingBody = this.bodies[i];
                    }
                }
            }

            // If the ray intersected a body, we alter the maxDistance to be equal to the distance of the body.
            if (bodyCollisionDistance != null)
                maxDistance = (float)bodyCollisionDistance;

            Vector3 startPoint = ray.Position;

            float x, y, z, height;
            x = y = z = height = 0;

            Vector3 currentPoint = startPoint;

            height = this.scene.GetHeightFromXZ(currentPoint.X, currentPoint.Z);

            // If maxDistance is infinitely large, we follow the ray until it reaches the
            // end of the scene, approx. 1 centimeters at a time.
            bool terrainCollision = false;
            if (float.IsInfinity(maxDistance))
            {
                Vector3 dir = ray.Direction;
                dir.Normalize();
                while (height >= 0 && !terrainCollision)
                {
                    currentPoint += ray.Direction * 0.01f;

                    height = this.scene.GetHeightFromXZ(currentPoint.X, currentPoint.Z);
                    if (currentPoint.Y <= height)
                    {
                        intersectingBody = null;
                        terrainCollision = true;
                        terrainIntersection = new Vector3(currentPoint.X, height, currentPoint.Z);
                    }
                }
            }
            else
            {
                Vector3 endPoint = startPoint + (ray.Direction * maxDistance);
                float rayDistance;
                Vector3.Distance(ref startPoint, ref endPoint, out rayDistance);

                float factor = 0f;
                float factorStep = 1f / rayDistance;

                while (height >= 0 && !terrainCollision && factor <= 1)
                {
                    currentPoint.X = MathHelper.Lerp(startPoint.X, endPoint.X, factor);
                    currentPoint.Y = MathHelper.Lerp(startPoint.Y, endPoint.Y, factor);
                    currentPoint.Z = MathHelper.Lerp(startPoint.Z, endPoint.Z, factor);

                    height = this.scene.GetHeightFromXZ(currentPoint.X, currentPoint.Z);
                    if (currentPoint.Y <= height)
                    {
                        intersectingBody = null;
                        terrainCollision = true;
                        terrainIntersection = new Vector3(currentPoint.X, height, currentPoint.Z);
                    }

                    factor += factorStep;
                }
            }

            if (!terrainCollision && intersectingBody == null)
                return RayIntersectionType.None;
            else
                return (terrainCollision ? RayIntersectionType.Terrain : RayIntersectionType.Body);
        }

        private void ApplyLogics(float timeSinceLastUpdate)
        {
            for (int i = 0; i < this.bodies.Count; i++)
            {
                for (int j = 0; j < this.logic.Count; j++)
                {
                    this.logic[j].ApplyOnBody(timeSinceLastUpdate, this.bodies[i]);
                }
            }
        }

        private void CollisionCheck()
        {
            // Terrain collision

            float terrainHeight;
            for (int i = 0; i < this.bodies.Count; i++)
            {
                // Find the terrain height at the bodys position.
                float height = (this.bodies[i].boundingBox.Max.Y - this.bodies[i].boundingBox.Min.Y);
                terrainHeight = this.scene.GetHeightFromXZ(this.bodies[i].position.X, this.bodies[i].position.Z);

                // If the bottom of the body is less or equal to the terrain height, it collides with the terrain.
                if (terrainHeight >= (this.bodies[i].position.Y - (height / 2)))
                {
                    this.bodies[i].position.Y = terrainHeight + (height / 2);
                    this.bodies[i].isOnGround = true;
                }
                else
                {
                    this.bodies[i].isOnGround = false;
                }
            }

            // Body-Body collision
            BoundingBox boxA, boxB;
            bool result;
            for (int i = 0; i < this.bodies.Count - 1; i++)
            {
                boxA = this.bodies[i].boundingBox;
                boxA.Min += this.bodies[i].position;
                boxA.Max += this.bodies[i].position;
                for (int j = i + 1; j < this.bodies.Count; j++)
                {
                    boxB = this.bodies[j].boundingBox;
                    boxB.Min += this.bodies[j].position;
                    boxB.Max += this.bodies[j].position;

                    boxA.Intersects(ref boxB, out result);
                    if (result)
                    {
                        this.bodies[i].RaiseBodyCollided(this.bodies[j]);
                        this.bodies[j].RaiseBodyCollided(this.bodies[i]);
                    }
                }
            }

            // Body-Plane collision
            PlaneIntersectionType intersectionType;
            if (this.collisionPlanes != null)
            {
                for (int i = 0; i < this.bodies.Count; i++)
                {
                    boxA = this.bodies[i].boundingBox;
                    boxA.Min += this.bodies[i].position;
                    boxA.Max += this.bodies[i].position;

                    for (int j = 0; j < this.collisionPlanes.Length; j++)
                    {
                        boxA.Intersects(ref this.collisionPlanes[j], out intersectionType);

                        // If intersectionType is anything other than Front, we are either intersecting the plane or
                        // standing behind it, which are both things we want to avoid.
                        if (intersectionType != PlaneIntersectionType.Front)
                        {
                            Vector3 negatedPlaneNormal, absolutePlaneNormal;
                            Vector3.Negate(ref this.collisionPlanes[j].Normal, out negatedPlaneNormal);
                            absolutePlaneNormal = new Vector3(
                                Math.Abs(this.collisionPlanes[j].Normal.X),
                                Math.Abs(this.collisionPlanes[j].Normal.Y),
                                Math.Abs(this.collisionPlanes[j].Normal.Z));
                            // Find the position of the plane, which is just defined as the distance from the world origin.
                            Vector3 planePosition = negatedPlaneNormal * this.collisionPlanes[j].D;

                            // By multiplying the bodys position with the absolute value of the plane normal, we will be given
                            // a vector that tells us how far into the plane that the body has traveled.
                            // The distance is calculated between this vector and the planes position. An extra distance offset of 0.01 is applied
                            // To avoid having bodies standing ON the plane boundary.

                            // First we need to find how much of the body has passed the plane.
                            Vector3 bodySize = this.bodies[i].boundingBox.Max - this.bodies[i].boundingBox.Min;
                            float dist = Vector3.Distance(
                                planePosition,
                                (this.bodies[i].position * absolutePlaneNormal) - ((bodySize * 0.5f) * this.collisionPlanes[j].Normal)) + 0.01f;

                            // Move the body 'dist' amount in the direction of the plane normal.
                            this.bodies[i].position += dist * this.collisionPlanes[j].Normal;
                            this.bodies[i].stateChanged = true;
                        }
                    }
                }
            }
        }

        private void CalculatePosition(float timeSinceLastUpdate)
        {
            Vector3 acceleration;
            for (int i = 0; i < this.bodies.Count; i++)
            {
                // Calculate acceleration.
                acceleration = (this.bodies[i].force + this.bodies[i].internalForce) / this.bodies[i].mass;

                // Update velocity.
                this.bodies[i].velocity += acceleration * timeSinceLastUpdate;

                // Calculate next position.
                Vector3 currentPosition = this.bodies[i].position;
                Vector3 nextposition = currentPosition + this.bodies[i].velocity;

                // Calculate the height of the terrain at the next position, and determine, based on the distance traveled
                // in relation to the time difference, if the body should be allowed to have the new position or not.
                // This is used to avoid letting bodies climb steep hills.
                float nextHeight = this.scene.GetHeightFromXZ(nextposition.X, nextposition.Z);
                float distanceTraveled = Math.Abs((nextposition - currentPosition).Length());
                float currentHeight = this.scene.GetHeightFromXZ(currentPosition.X, currentPosition.Z);
                if (!this.bodies[i].isOnGround || (nextHeight - currentHeight) < (this.bodies[i].maxStepSize / distanceTraveled))
                {
                    this.bodies[i].position = nextposition;
                    this.bodies[i].stateChanged = true;
                }


            }
        }

        /// <summary>
        /// Provides data for the BodyCollided event.
        /// </summary>
        public class BodyCollidedEventArgs : EventArgs
        {
            private Body other;

            /// <summary>
            /// Creates a new instance of this class.
            /// </summary>
            /// <param name="other">The body that the sender is colliding with</param>
            public BodyCollidedEventArgs(Body other)
            {
                this.other = other;
            }

            /// <summary>
            /// The body with which the sender is colliding.
            /// </summary>
            public Body Other
            {
                get { return this.other; }
            }
        }
    }
}
